#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/SceneObject.h>
#include <Inventor/nodes/SoSphere.h>


#include "ModelPoseMarkerSensorVisualisation.h"

using namespace MMM;

ModelPoseMarkerSensorVisualisation::ModelPoseMarkerSensorVisualisation(ModelPoseSensorPtr sensor, VirtualRobot::RobotPtr robot, VirtualRobot::CoinVisualizationPtr visualization, SoSeparator* sceneSep) :
    SensorVisualisation(sceneSep),
    sensor(sensor),
    robot(robot)
{
    SoSeparator* robotSep = new SoSeparator();

    if (visualization) {
        SoNode* visualisationNode = visualization->getCoinVisualization();
        if (visualisationNode) robotSep->addChild(visualisationNode);
    }

    markerSep = new SoSeparator();
    robotSep->addChild(markerSep);

    setVisualisationSep(robotSep);
}

std::string ModelPoseMarkerSensorVisualisation::getType() {
    return sensor->getType();
}

int ModelPoseMarkerSensorVisualisation::getPriority() {
    return sensor->getPriority();
}

SoSeparator* ModelPoseMarkerSensorVisualisation::createLabel(const std::string &label) {
    SoSeparator* labelSep = new SoSeparator;
    Eigen::Vector3f labelVector;
    labelVector << 0, 30, 0;
    Eigen::Matrix4f localPose = Eigen::Matrix4f::Identity();
    localPose.block(0, 3, 3, 1) += labelVector;
    localPose.block(0, 0, 3, 3) *= 2;
    labelSep->addChild(VirtualRobot::CoinVisualizationFactory::getMatrixTransform(localPose));
    labelSep->addChild(VirtualRobot::CoinVisualizationFactory::CreateBillboardText(label));

    return labelSep;
}

SoSeparator* ModelPoseMarkerSensorVisualisation::createSphere(Eigen::Matrix4f globalPose) {
    SoSeparator* sphereSep = new SoSeparator;
    SoMaterial* marked = new SoMaterial;
    marked->ambientColor.setValue(1.0f, 0.2f, 0.2f);
    marked->diffuseColor.setValue(1.0f, 0.2f, 0.2f);
    marked->specularColor.setValue(0.5f, 0.5f, 0.5f);
    sphereSep->addChild(marked);
    sphereSep->addChild(VirtualRobot::CoinVisualizationFactory::getMatrixTransform(globalPose));
    SoSphere* sphere = new SoSphere;
    sphere->radius = 10;
    sphereSep->addChild(sphere);

    return sphereSep;
}

void ModelPoseMarkerSensorVisualisation::setPose(SoSeparator* s, Eigen::Matrix4f globalPose) {
    SoMatrixTransform* mt = (SoMatrixTransform*)s->getChild(1);
    SbMatrix m_(reinterpret_cast<SbMat*>(globalPose.data()));
    mt->matrix.setValue(m_);
}

void ModelPoseMarkerSensorVisualisation::update(float timestep, float delta) {
    update(sensor->getDerivedMeasurement(timestep, delta));
}

void ModelPoseMarkerSensorVisualisation::update(float timestep) {
    update(sensor->getDerivedMeasurement(timestep));
}

void ModelPoseMarkerSensorVisualisation::update(ModelPoseSensorMeasurementPtr measurement) {
    if (!measurement) return;
    robot->setGlobalPose(measurement->getRootPose());

    std::vector<VirtualRobot::SensorPtr> sensors = robot->getSensors();

    for (const auto &sensor : sensors) {
        if (!sensor->getName().empty()) {
            if (markerSphereSeps.find(sensor->getName()) != markerSphereSeps.end()) {
                SoSeparator* sphereSep = markerSphereSeps[sensor->getName()];
                setPose(sphereSep, sensor->getGlobalPose());
            } else {
                // not found, create sphere
                SoSeparator* sphereSep = createSphere(sensor->getGlobalPose());
                sphereSep->addChild(createLabel(sensor->getName()));
                sphereSep->ref();
                markerSep->addChild(sphereSep);
                markerSphereSeps[sensor->getName()] = sphereSep;
            }
        }
    }
}
